import time

import cv2
import numpy as np
# termination criteria
from scipy.io import loadmat

data = loadmat('mtx.mat')
mtx, newcameramtx, dist, roi = data['mtx'], data['new_mtx'], data['dist'], data['roi']
cap = cv2.VideoCapture(0)  # 创建一个 VideoCapture 对象
# cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
# cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
total_time = []
while cap.isOpened():  # 循环读取每一帧0
    ret_flag, img = cap.read()
    # undistort
    tic = time.time()
    dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
    x, y, w, h = roi
    dst = dst[y:y + h, x:x + w]
    # dst = cv2.resize(dst[y:y + h, x:x + w], (320, 240))
    # toc = time.time()
    # total_time.append(toc - tic)
    cv2.imshow('dst', dst)
    # crop the image

    k = cv2.waitKey(1)
    if k == ord("p"):
        print(dst.shape)
        print(np.mean(total_time))
    if k == 27:
        break
cap.release()
cv2.destroyAllWindows()
